#include "SOEMMaster.h"
#include <iostream>
#include <cstring>

// 包含SOEM头文件
extern "C" {
#include "ethercattype.h"
#include "nicdrv.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatdc.h"
#include "ethercatcoe.h"
#include "ethercatfoe.h"
#include "ethercatconfig.h"
#include "ethercatprint.h"
}

// 外部变量声明，这些在SOEM中是全局变量
extern ec_slavet ec_slave[];
extern ec_groupt ec_group[];
extern int expectedWKC;
extern int wkc;
extern boolean inOP;
extern boolean needlf;
extern volatile int wkc;
extern int currentgroup;

SOEMMaster::SOEMMaster()
    : ifname(nullptr), inOP(false), needlf(false), expectedWKC(0), wkc(0), currentgroup(0),
      IOmap(nullptr), forceByteAlignment(false) {
}

SOEMMaster::~SOEMMaster() {
    shutdown();
    if (ifname) {
        delete[] ifname;
        ifname = nullptr;
    }
    if (IOmap) {
        delete[] IOmap;
        IOmap = nullptr;
    }
}

bool SOEMMaster::init(const std::string& ifname_str) {
    // 复制网卡名称
    ifname = new char[ifname_str.length() + 1];
    strcpy(ifname, ifname_str.c_str());
    
    // 初始化SOEM，绑定socket到网卡
    if (ec_init(ifname)) {
        std::cout << "SOEM initialized on " << ifname << std::endl;
        return true;
    } else {
        std::cerr << "Failed to initialize SOEM on " << ifname << std::endl;
        return false;
    }
}

bool SOEMMaster::configureSlaves() {
    // 查找并自动配置从站
    if (ec_config_init(FALSE) > 0) {
        std::cout << ec_slavecount << " slaves found and configured." << std::endl;
        
        // 映射IO
        if (forceByteAlignment) {
            IOmap = new uint8_t[4096]; // 分配足够大的内存
            ec_config_map_aligned(IOmap);
        } else {
            IOmap = new uint8_t[4096]; // 分配足够大的内存
            ec_config_map(IOmap);
        }
        
        // 配置分布式时钟
        ec_configdc();
        
        return true;
    } else {
        std::cerr << "No slaves found!" << std::endl;
        return false;
    }
}

bool SOEMMaster::activate() {
    std::cout << "Requesting operational state for all slaves" << std::endl;
    
    // 计算预期的工作计数器
    expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
    std::cout << "Calculated workcounter " << expectedWKC << std::endl;
    
    // 设置主站状态为OPERATIONAL
    ec_slave[0].state = EC_STATE_OPERATIONAL;
    
    // 发送一个有效的过程数据，使从站输出正常
    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);
    
    // 请求所有从站进入OP状态
    ec_writestate(0);
    
    // 等待所有从站进入OP状态
    int chk = 200;
    do {
        ec_send_processdata();
        ec_receive_processdata(EC_TIMEOUTRET);
        ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
    } while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
    
    if (ec_slave[0].state == EC_STATE_OPERATIONAL) {
        std::cout << "Operational state reached for all slaves." << std::endl;
        inOP = true;
        return true;
    } else {
        std::cerr << "Not all slaves reached operational state." << std::endl;
        ec_readstate();
        for (int i = 1; i <= ec_slavecount; i++) {
            if (ec_slave[i].state != EC_STATE_OPERATIONAL) {
                std::cerr << "Slave " << i << " State=0x" << std::hex << ec_slave[i].state
                          << " StatusCode=0x" << ec_slave[i].ALstatuscode << std::dec
                          << " : " << ec_ALstatuscode2string(ec_slave[i].ALstatuscode) << std::endl;
            }
        }
        return false;
    }
}

bool SOEMMaster::receiveProcessData() {
    wkc = ec_receive_processdata(EC_TIMEOUTRET);
    return (wkc >= expectedWKC);
}

bool SOEMMaster::sendProcessData() {
    ec_send_processdata();
    return true;
}

bool SOEMMaster::checkMasterState() {
    // SOEM没有直接的主站状态检查，可以通过检查从站状态来间接实现
    if (inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) {
        ec_group[currentgroup].docheckstate = FALSE;
        ec_readstate();
        for (int slave = 1; slave <= ec_slavecount; slave++) {
            if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) {
                ec_group[currentgroup].docheckstate = TRUE;
                if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) {
                    std::cout << "ERROR : slave " << slave << " is in SAFE_OP + ERROR, attempting ack." << std::endl;
                    ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
                    ec_writestate(slave);
                } else if (ec_slave[slave].state == EC_STATE_SAFE_OP) {
                    std::cout << "WARNING : slave " << slave << " is in SAFE_OP, change to OPERATIONAL." << std::endl;
                    ec_slave[slave].state = EC_STATE_OPERATIONAL;
                    ec_writestate(slave);
                } else if (ec_slave[slave].state > EC_STATE_NONE) {
                    if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) {
                        ec_slave[slave].islost = FALSE;
                        std::cout << "MESSAGE : slave " << slave << " reconfigured" << std::endl;
                    }
                } else if (!ec_slave[slave].islost) {
                    // 重新检查状态
                    ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
                    if (ec_slave[slave].state == EC_STATE_NONE) {
                        ec_slave[slave].islost = TRUE;
                        std::cout << "ERROR : slave " << slave << " lost" << std::endl;
                    }
                }
            }
            if (ec_slave[slave].islost) {
                if (ec_slave[slave].state == EC_STATE_NONE) {
                    if (ec_recover_slave(slave, EC_TIMEOUTMON)) {
                        ec_slave[slave].islost = FALSE;
                        std::cout << "MESSAGE : slave " << slave << " recovered" << std::endl;
                    }
                } else {
                    ec_slave[slave].islost = FALSE;
                    std::cout << "MESSAGE : slave " << slave << " found" << std::endl;
                }
            }
        }
        if (!ec_group[currentgroup].docheckstate) {
            std::cout << "OK : all slaves resumed OPERATIONAL." << std::endl;
        }
    }
    return true;
}

bool SOEMMaster::checkDomainState() {
    // SOEM没有直接的域状态检查，可以通过工作计数器来间接实现
    if (wkc >= expectedWKC) {
        return true;
    } else {
        std::cerr << "Warning: Working counter too low. Expected: " << expectedWKC << ", Got: " << wkc << std::endl;
        return false;
    }
}

bool SOEMMaster::checkSlaveConfigState() {
    // 在checkMasterState中已经实现了从站状态检查
    return true;
}

void SOEMMaster::shutdown() {
    if (inOP) {
        std::cout << "Request init state for all slaves" << std::endl;
        ec_slave[0].state = EC_STATE_INIT;
        // 请求所有从站进入INIT状态
        ec_writestate(0);
        inOP = false;
    }
    
    // 关闭SOEM，关闭socket
    ec_close();
    std::cout << "SOEM EtherCAT master shut down" << std::endl;
}

void SOEMMaster::cyclicTask() {
    // 发送过程数据
    sendProcessData();
    
    // 接收过程数据
    receiveProcessData();
    
    // 检查主站状态
    checkMasterState();
    
    // 这里可以添加读写过程数据的代码
    // 例如：处理输入输出数据
    
    // 等待下一个周期
    osal_usleep(CYCLE_TIME_US);
}

void SOEMMaster::checkSlaveStatus() {
    if (inOP) {
        if ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate) {
            // 在checkMasterState中已经实现了从站状态检查
            checkMasterState();
        }
    }
}